#include "pwm.h"
#include "main.h"
//#include "Logomatic_V2.h"
static unsigned int pwm_counts_filtered = 0;
static unsigned int pwm_target = 0;
static unsigned int button_target=0;
static unsigned int Slow_ramp=1;
static unsigned int pwm_counts_base=0;

#define pwm_limit 4000
void pwm_config(void);
/*
typedef struct pwmStruct {
  int duty_target;
  int duty_actual;
  char slow_ramp;
};
*/
typedef struct PID_data
{
    unsigned int pid_delay;
    float PID_output;
    unsigned int PID_duty_unstable;
    unsigned int PID_duty_stable;
    bool PID_stable;
    bool PID_pushInPush ;
    float PID_Prev_SetPoint;
    
    float PID_current_brake_ratio;
    float PID_speed_ratio;
    float PID_current_ratio;
    float PID_previous_ratio;
    unsigned int PID_manual_output;
    int PID_pressed_delay;
    int PID_timeout_count;
} PID_data;

typedef struct pwmStruct {
  int count;
  int index;
  int outdex;
  int capacity;
  int * values;
 } pwmStruct;
enum { BUFFER_GUARD = -999};

PID_t pid_init(void)
{
  PID_t self = (PID_data*)calloc(1, sizeof(PID_data));
  self->pid_delay = 0;
  self->PID_output=0.0f;
  self->PID_duty_unstable=0;
  self->PID_duty_stable=0;
  self->PID_stable=false;
  self->PID_pushInPush=false;
  self->PID_Prev_SetPoint=1.0f;
  self->PID_current_brake_ratio=0.0f;
  self->PID_speed_ratio=2.0f;
  self->PID_current_ratio=0.0f;
  self->PID_previous_ratio=0.0f;
  self->PID_manual_output=0;
  self->PID_pressed_delay=0;
  self->PID_timeout_count=0;
}
PWM_t pwm_Create(int capacity)
{
  PWM_t self = (pwmStruct*)calloc(capacity, sizeof(pwmStruct));
  self->capacity = capacity;
  self->values = (int*)calloc(capacity+1, sizeof(int));
  self->values[capacity]=BUFFER_GUARD;
  return self;
}
void  pwm_init(void)
{
    PCONP |= 1<<5;
//    PINSEL0 |= 0B10<<16;  //PWM4   ON P0.8
    PINSEL0 = ( PINSEL0 & (~(1<<16)))|(1<<17);
    
    PWMPR=0;   //pclk=72Mhz   pwm clock=72M/36=2MHZ
    PWMMCR = 1<<1;
    PWMMR0=4000; // 2M/100=20KHZ PWM
    PWMMR4=0;
    PWMPCR |= 1<<12;  //ENABLE PWM OUTPUT4 AND SINGLE EDGE CONTROL
    PWMLER = (1<<0)|(1<<4);
    PWMTCR = 1|(1<<3);//ENABLE COUNTER AND PWM
}
void  pwm_config(void)
{
// same as pwm_init except it doesn't set PWMR4
    PCONP |= 1<<5;
    PINSEL0 = ( PINSEL0 & (~(1<<16)))|(1<<17);
    
    PWMPR=0;   //pclk=72Mhz   pwm clock=72M/36=2MHZ
    PWMMCR = 1<<1;
    PWMMR0=4000; // 2M/100=20KHZ PWM
//    PWMMR4=0;
    PWMPCR |= 1<<12;  //ENABLE PWM OUTPUT4 AND SINGLE EDGE CONTROL
    PWMLER = (1<<0)|(1<<4);
    PWMTCR = 1|(1<<3);//ENABLE COUNTER AND PWM
}
void update_pwm(void)
{
  unsigned int pwm_counts = 0;
  unsigned int pwm_button_counts = 0;
//  pwm_config(); // this was added to ensure the pwm is properly configured every time the pwm is updated

  pwm_counts = 40*pwm_target;
  pwm_button_counts=40*button_target;

  if (pwm_target==0)
  {
    pwm_counts_filtered=0;
    pwm_counts_base=0;
    //motor_run=0;
  }
  else
    pwm_counts_base=400;   //start form 10%
  if (pwm_counts > pwm_counts_filtered+pwm_counts_base) 
  {
//    ++pwm_counts_filtered;
    if(Slow_ramp)
    {  
      if(pwm_button_counts>pwm_counts_filtered+pwm_counts_base) 
      {
        pwm_counts_filtered+=8;
      }
      else
      {
        pwm_counts_filtered+=1;
      }
    }
    else
      pwm_counts_filtered+=8;
  }
//  else if (pwm_counts > pwm_counts_filtered) 
//  {
//   if (pwm_counts_filtered > 0)
//      --pwm_counts_filtered;
//   else
//      pwm_counts_filtered = 0;
//  }
  else
  {
    pwm_counts_filtered = pwm_counts -pwm_counts_base;
  }
  if ((pwm_counts_filtered+pwm_counts_base) > pwm_limit) {
    pwm_counts_filtered = pwm_limit - pwm_counts_base;
  }
 // if (pwm_counts_filtered > 0) {
      if(pwm_counts_filtered==0)
      {
        PWMMR4=1;  
        //motor_run=0;
      }
      else
      {
        //motor_run=1;
        PWMMR4=(pwm_counts_filtered+pwm_counts_base);
      }
      PWMLER = 1<<4;  //ENABLE LATCH RESIGTER
//  }
}
void set_pwm(unsigned int _pwm_duty)
{
  Slow_ramp=1;
  pwm_target = _pwm_duty;
  button_target=0;
}

void set_pwm_button(unsigned pwm_duty)
{
  Slow_ramp=1;
  button_target = pwm_duty;
  pwm_target=100;
}

void set_pwm_FORCED(unsigned int _pwm_duty)
{
  Slow_ramp=0;
  button_target=0;
  pwm_target = _pwm_duty;
  unsigned int pwm_counts = 0;
//  pwm_counts = 40*_pwm_duty;
//  if (pwm_counts > pwm_limit) {
//    pwm_counts = pwm_limit;
//  }
//  if(pwm_counts==0)
//
//  {
//    PWMMR4=1;  
//  }
//  else
//  {
//    PWMMR4=pwm_counts;
//  }
//  PWMLER = 1<<4;  //ENABLE LATCH RESIGTER
//  pwm_counts_filtered = pwm_counts-800;
//  pwm_counts_filtered = pwm_counts_filtered>0 ? pwm_counts_filtered : 0;
}
unsigned int get_pwm(void)
  {
   return pwm_counts_filtered/40;
}
